﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Denting
{
    
    public struct RotateAngleXY
    {
        public double xRotate;
        public double yRotate;
        public void show()
        {
            Console.WriteLine("XR={0}   |YR={1}   ", xRotate, yRotate);
            
        }
    }

    public struct VectorXYZ
    {
        public double x;
        public double y;
        public double z;
        public void show()
        {
            Console.WriteLine("XT={0}   |YT={1}   |ZT={2}   |", x, y, z);
            Console.WriteLine("M={0}", Math.Sqrt(x * x + y * y + z * z));
        }
    }

    class TRS
    {
        const double PI = 3.1415926535897932384;
        public VectorXYZ GetTranslateXYZ(Node nBegin, Node nEnd)
        {
            VectorXYZ t = new VectorXYZ();
            t.x = nEnd.X_position - nBegin.X_position;
            t.y = nEnd.Y_position - nBegin.Y_position;
            t.z = nEnd.Z_position - nBegin.Z_position;
            return t;
        }
        
        //该函数目前实现有问题
        public RotateAngleXY GetRotateAngleXY(Node nBegin,Node nEnd)
        {
            VectorXYZ t = GetTranslateXYZ(nBegin, nEnd);
            RotateAngleXY r = new RotateAngleXY();
            
            r.xRotate = 360-Math.Acos(Math.Sqrt(t.x * t.x  + t.z * t.z) / Math.Sqrt(t.x * t.x + t.y * t.y + t.z * t.z)) * 180 / PI;
            r.yRotate = Math.Acos(t.z / Math.Sqrt(t.x * t.x + t.z * t.z)) * 180 / PI;
            if (t.z < 0)
            {
                r.xRotate = 360 - r.xRotate;
                r.yRotate = 360 - r.yRotate;
            }
            return r;
        }
        public RotateAngleXY GetRotateAngleXY(VectorXYZ t)
        {
            RotateAngleXY r = new RotateAngleXY();

            r.xRotate = Math.Acos(t.y / Math.Sqrt(t.x * t.x + t.y * t.y + t.z * t.z)) * 180 / PI - 90;
            r.yRotate = -Math.Acos(t.z / Math.Sqrt(t.x * t.x + t.z * t.z)) * 180 / PI;
            if (t.x>0)
            {
                r.yRotate = Math.Acos(t.z / Math.Sqrt(t.x * t.x + t.z * t.z)) * 180 / PI;
            }
  
            //r.yRotate = 0;
            return r;
        }

        public VectorXYZ GetVectorX(VectorXYZ a, VectorXYZ b)
        {
            VectorXYZ c = new VectorXYZ();
            c.x = a.y * b.z - a.z * b.y;
            c.y = a.z * b.x - a.x * b.z;
            c.z = a.x * b.y - a.y * b.x;
            double m = Math.Sqrt(c.x * c.x + c.y * c.y + c.z * c.z);
            c.x /= m;
            c.y /= m;
            c.z /= m;
            return c;
        }
    }
}
